//ATTENTION: THIS FILE IS UNDER VERSION CONTROL. FEEL FREE TO MAKE ANY CHANGES
//I can revert as needed.

#include "WPILib.h"
#include "defines.h"
#include "bindings.h"
#include <math.h>

class RobotDemo : public SimpleRobot
{
	Joystick* leftJoystick;
	Joystick* rightJoystick;
	Jaguar* leftJag;
	Jaguar* rightJag;
	DriverStationLCD* lcd;
	Jaguar* Shooter;
	Jaguar* Shooter2;
	Victor* Conveyor;
	Victor* Grabber;
	Victor* Arm;

	RobotDrive* myRobot; // Not used? *MOD
	
	bool ConveyorOn;
	bool ConveyorForward;
	bool onestick;
	bool stickpressed;
public:
	RobotDemo(void)
	{
		leftJoystick = new Joystick(1);
		rightJoystick = new Joystick(2);
		leftJag = new Jaguar(LEFT_DRIVE_PORT);
		rightJag = new Jaguar(RIGHT_DRIVE_PORT);
		Shooter = new Jaguar(SHOOTER_PORT);
		Shooter2 = new Jaguar(SHOOTER2_PORT);
		Conveyor = new Victor(CONVEYOR_PORT);
		Grabber = new Victor(GRABBER_PORT);
		Arm = new Victor(ARM_PORT);
		ConveyorOn = true;
		ConveyorForward = true; 		
		onestick = false;
		stickpressed = false;
		lcd = DriverStationLCD::GetInstance();
		
		
	}

	void Autonomous(void)
	{
		GetWatchdog().SetEnabled(false);
#ifdef DO_AUTONOMOUS
		rightJag->Set(AUTONOMOUS_MOVEMENT_SPEED);
		leftJag->Set(-AUTONOMOUS_MOVEMENT_SPEED);
		Wait(AUTONOMOUS_MOVEMENT_TIME);
		rightJag->Set(0);
		leftJag->Set(0);
		Shooter->Set(AUTONOMOUS_SHOT_SPEED*SHOT_SPEED_MULTIPLIER);
		Shooter2->Set(-AUTONOMOUS_SHOT_SPEED);
		Wait(2);
		Conveyor->Set(-CONVEYOR_SPEED);
		while(IsAutonomous()) ;
		Shooter->Set(0);
		Shooter2->Set(0);
		Conveyor->Set(0);
#endif
	}

	void OperatorControl(void)
	{
		GetWatchdog().SetEnabled(false);
		Grabber->Set(GRABBER_SPEED);
		while (IsOperatorControl())
		{
			float leftSpeed = 0;
			float rightSpeed = 0;

			leftSpeed = leftJoystick->GetY();
			rightSpeed = leftJoystick->GetRawAxis(4);

			if (leftJoystick->GetRawButton(ONE_STICK_BUTTON) && !stickpressed)
				onestick = !onestick;
			stickpressed = leftJoystick->GetRawButton(ONE_STICK_BUTTON);
			if (onestick)
			{
				if (fabs(leftJoystick->GetX()) > fabs(leftJoystick->GetY()))
				{
					leftSpeed = -leftJoystick->GetX();
					rightSpeed = -leftJoystick->GetX();
				}
				else
				{
					leftSpeed = -leftJoystick->GetY();
					rightSpeed = leftJoystick->GetY();
				}
			}
			else
			{
#ifdef PS2_CONTROLLER
				if(leftJoystick->GetRawAxis(5)!=0.0f)
				{
					leftSpeed = -leftJoystick->GetRawAxis(5) * DPAD_MULTIPLIER;
					rightSpeed = leftJoystick->GetRawAxis(5) * DPAD_MULTIPLIER;
				}
				else
				{
				leftSpeed = -leftJoystick->GetY();
				rightSpeed = leftJoystick->GetRawAxis(3);
				}
			
#else
				if (leftJoystick->GetRawButton(STRAIGHT_FORWARD_BUTTON) || leftJoystick->GetRawButton(STRAIGHT_BACKWARD_BUTTON))
				{
					if (leftJoystick->GetRawButton(STRAIGHT_FORWARD_BUTTON))
					{
						leftSpeed = DPAD_MULTIPLIER;
						rightSpeed = -DPAD_MULTIPLIER;
					}
					else if (leftJoystick->GetRawButton(STRAIGHT_BACKWARD_BUTTON))
					{
						leftSpeed = -DPAD_MULTIPLIER;
						rightSpeed = DPAD_MULTIPLIER;
					}
				}
				else
				{
					leftSpeed = -leftJoystick->GetY();
					rightSpeed = leftJoystick->GetRawAxis(3);
				}	
#endif
			}
			leftJag->Set(leftSpeed);
			rightJag->Set(rightSpeed);
			if (leftJoystick->GetRawButton(SHOOTER_FORWARD_BUTTON))
			{
				Shooter->Set(SHOT_SPEED*SHOT_SPEED_MULTIPLIER);
				Shooter2->Set(-SHOT_SPEED);
			}
			else if (leftJoystick->GetRawButton(SHOOTER_BACKWARD_BUTTON))
			{
				Shooter->Set(SHOT_SPEED_2*SHOT_SPEED_MULTIPLIER);
				Shooter2->Set(-SHOT_SPEED_2);
			}
			else
			{
				Shooter->Set(0);
				Shooter2->Set(0);
			}	
			if (leftJoystick->GetRawButton(GRABBER_FORWARD_BUTTON))
			{
				Grabber->Set(GRABBER_SPEED);
			}
			else if (leftJoystick->GetRawButton(GRABBER_BACKWARD_BUTTON)) //Reverse button for conveyor and roller
			{
				Grabber->Set(-GRABBER_SPEED);
			}
			else if (leftJoystick->GetRawButton(GRABBER_RESET_BUTTON))
			{
				Grabber->Set(0);
			} 
			
			float mult = 0;
			if (leftJoystick->GetRawButton(GRABBER_BACKWARD_BUTTON))
				mult = -1;
			if (leftJoystick->GetRawButton(GRABBER_FORWARD_BUTTON))
				mult = 1;
			Conveyor->Set(CONVEYOR_SPEED * mult);
			Grabber->Set(GRABBER_SPEED * mult);
			//Arm->SetSpeed(ARM_SPEED * leftJoystick->GetRawAxis(6)); // Never 0, alway either +speed or -speed? *MOD
			
			lcd-> Printf(DriverStationLCD::kUser_Line1, 1, "A1: %f", leftJoystick->GetX());
			lcd-> Printf(DriverStationLCD::kUser_Line2, 1, "A2: %f", leftJoystick->GetY());
			lcd-> Printf(DriverStationLCD::kUser_Line3, 1, "A3: %f", leftJoystick->GetRawAxis(3));
			lcd-> Printf(DriverStationLCD::kUser_Line4, 1, "A4: %f", leftJoystick->GetRawAxis(4));
			lcd-> Printf(DriverStationLCD::kUser_Line5, 1, onestick ? "One Stick Mode" : "Two Stick Mode");
			lcd->UpdateLCD();
			Wait(0.005);
		}
	}
};

START_ROBOT_CLASS(RobotDemo);

